/**
 * @file:
 *   mctrl.c
 * @author:
 *   Kristopher Bechamp
 * @date:
 *   2011.10.15
 *
 * @brief:
 *   The interface for motor control is provided by a SN75441
 *   H-Bridge IC. There are a total of 6 connections to the 
 *   chip; 4 digital I/O drive control lines and 2 I/O lines
 *   that control the speed. The speed control lines can either
 *   be powered by the rails at an all time full speed setting 
 *   or it may be controlled as a varying speed through PWM.
 *
 *   The functions are broken down into induvidual wheel functions
 *   to help avoid "Shoot-Through" of the H-Bridge.
 *
 *   THIS LIBRARY FILE IS SOLEY CONSERNED WITH THE WHEEL CONTROL
 *   AND DOES NOT ATTEMPT TO CONTROL SPEED; THAT IS PROVIDED BY AN
 *   EXTERNAL SOURCE.
 */
#include "mctrl.h"
#include "launchpad.h"

/**
 * Motor Function Delcarations -- PRIVATE
 */
static void MCTRL_RightForward(void);
static void MCTRL_LeftForward(void);
static void MCTRL_RightBackward(void);
static void MCTRL_LeftBackward(void);
static void MCTRL_LeftBreak(void);
static void MCTRL_RightBreak(void);

/**
 * Motor Function Definitions
 */
void MCTRL_Init(void) {
    // Set pins to output
    BITSET(MCTRL_DDR, MCTRL_RIGHT_FORWARD);
    BITSET(MCTRL_DDR, MCTRL_RIGHT_BACKWARD);
    BITSET(MCTRL_DDR, MCTRL_LEFT_FORWARD);
    BITSET(MCTRL_DDR, MCTRL_LEFT_BACKWARD);
    // Clear pins for safety
    BITCLR(MCTRL_PORT, MCTRL_RIGHT_FORWARD);
    BITCLR(MCTRL_PORT, MCTRL_RIGHT_BACKWARD);
    BITCLR(MCTRL_PORT, MCTRL_LEFT_FORWARD);
    BITCLR(MCTRL_PORT, MCTRL_LEFT_BACKWARD);
}

static void MCTRL_RightBreak(void) {
    BITCLR(MCTRL_PORT, MCTRL_RIGHT_FORWARD);
    BITCLR(MCTRL_PORT, MCTRL_RIGHT_BACKWARD);
}

static void MCTRL_RightForward(void) {
    BITSET(MCTRL_PORT, MCTRL_RIGHT_FORWARD);
    BITCLR(MCTRL_PORT, MCTRL_RIGHT_BACKWARD);
}

static void MCTRL_RightBackward(void) {
    BITSET(MCTRL_PORT, MCTRL_RIGHT_BACKWARD);
    BITCLR(MCTRL_PORT, MCTRL_RIGHT_FORWARD);
}

static void MCTRL_LeftBreak(void) {
    BITCLR(MCTRL_PORT, MCTRL_LEFT_FORWARD);
    BITCLR(MCTRL_PORT, MCTRL_LEFT_BACKWARD);
}

static void MCTRL_LeftForward(void) {
    BITSET(MCTRL_PORT, MCTRL_LEFT_FORWARD);
    BITCLR(MCTRL_PORT, MCTRL_LEFT_BACKWARD);
}

static void MCTRL_LeftBackward(void) {
    BITSET(MCTRL_PORT, MCTRL_LEFT_BACKWARD);
    BITCLR(MCTRL_PORT, MCTRL_LEFT_FORWARD);
}

/**
 * Exported Functions
 **/
void MCTRL_DriveForward(void) {
    MCTRL_LeftForward();
    MCTRL_RightForward();
}

void MCTRL_DriveLeft(void) {
    MCTRL_LeftBreak();
    MCTRL_RightForward();
}

void MCTRL_DriveRight(void) {
    MCTRL_RightBreak();
    MCTRL_LeftForward();
}

void MCTRL_DriveBackward(void) {
    MCTRL_RightBackward();
    MCTRL_LeftBackward();
}

void MCTRL_Stop(void) {
    MCTRL_LeftBreak();
    MCTRL_RightBreak();
}

void MCTRL_SpinLeft(void) {
    MCTRL_LeftBackward();
    MCTRL_RightForward();
}

void MCTRL_SpinRight(void) {
    MCTRL_RightBackward();
    MCTRL_LeftForward();
}
